#include "lidar_data_collect.h"

/**************************************************************************************
功能描述: 初始化
修改记录:
**************************************************************************************/
int LIDAR_DATA_COLLECT_C::Init()
{
    strPcdDirPath_ = objDataFile_.GetPcdDirPath();
    if (strPcdDirPath_.empty()) {
        ST_LOG_ERR("%s Directory Not Exist. Init Fail.", strPcdDirPath_.c_str());
        return -1;
    }
    
    strPlyDirPath_ = objDataFile_.GetPlyDirPath();
    if (strPlyDirPath_.empty()) {
        ST_LOG_ERR("%s Directory Not Exist. Init Fail.", strPlyDirPath_.c_str());
        return -1;
    }

    strBinDirPath_ = objDataFile_.GetBinDirPath();
    if (strBinDirPath_.empty()) {
        ST_LOG_ERR("%s Directory Not Exist. Init Fail.", strBinDirPath_.c_str());
        return -1;
    }

    strDrcDirPath_ = objDataFile_.GetDrcDirPath();
    if (strDrcDirPath_.empty()) {
        ST_LOG_ERR("%s Directory Not Exist. Init Fail.", strDrcDirPath_.c_str());
        return -1;
    }

    return 0;
}

/**************************************************************************************
功能描述: 数据采集
修改记录:
**************************************************************************************/
void LIDAR_DATA_COLLECT_C::DataCollect()
{
    MyCloud::Ptr pCloud(new MyCloud());
    objLidarMsgRec_.GetNormalPointsCloud(pCloud);

    if (pCloud->size()) {
        string strPlyFileName = SensorMsg2Ply(pCloud);
        if (0 != DracoEndoder(strPlyFileName)) {
            ST_LOG_ERR("%s Encoder Fail", strPlyFileName.c_str());
            return;
        } else {
            objDataFile_.DeleteFile((strPlyDirPath_ + strPlyFileName).c_str());
        }
    }
}

/**************************************************************************************
功能描述: 获取前缀
修改记录:
**************************************************************************************/
string LIDAR_DATA_COLLECT_C::GetPrefix(const string &lidarName) const
{
    string strPrefix;
    
    if (lidarName == HEAD_LEFT_RS16_LIDAR)
        strPrefix = "_rs16";
    else if (lidarName == HEAD_LEFT_RSBPEARL_LIDAR)
        strPrefix = "_rslbp";
    else if (lidarName == HEAD_RIGHT_RSBPEARL_LIDAR)
        strPrefix = "_rsrbp";
    else if (lidarName == MIDDLE_RS16_LIDAR)
        strPrefix = "_rs16";
    else if (lidarName == MIDDLE_RSBPEARL_LIDAR)
        strPrefix = "_rsbp";
    else
        strPrefix = "_null";

    return strPrefix;
}

/**************************************************************************************
功能描述: 将话题消息写入到pcd文件，并返回文件名
修改记录:
**************************************************************************************/
string LIDAR_DATA_COLLECT_C::SensorMsg2Pcd(const MyCloud::Ptr pCloud) const
{
    double tt = pCloud->header.stamp / 1000000.0;
    string fileNameBin   = std::to_string(tt) + strPrefix_ + "_bin.pcd";
    string fileNameAscii = std::to_string(tt) + strPrefix_ + "_ascii.pcd";

    pcl::PointCloud<pcl::PointXYZ> cloud;
    pcl::copyPointCloud(*pCloud, cloud);

    pcl::io::savePCDFileBinary(strPcdDirPath_ + fileNameBin, cloud);
    pcl::io::savePCDFileASCII(strPcdDirPath_ + fileNameAscii, cloud);
    
    usleep(10 * 1000);
    return fileNameBin;
}

/**************************************************************************************
功能描述: 将话题消息写入到ply文件，并返回文件名
修改记录:
**************************************************************************************/
string LIDAR_DATA_COLLECT_C::SensorMsg2Ply(const MyCloud::Ptr pCloud) const
{
    double tt = pCloud->header.stamp / 1000000.0;
    string fileName = std::to_string(tt) + strPrefix_ + ".ply";

    pcl::PointCloud<pcl::PointXYZ> cloud;
    pcl::copyPointCloud(*pCloud, cloud);
    
    pcl::io::savePLYFile(strPlyDirPath_ + fileName, cloud);
    usleep(10 * 1000);
    return fileName;
}

/**************************************************************************************
功能描述: 将话题消息写入到bin文件，并返回文件名
修改记录:
**************************************************************************************/
string LIDAR_DATA_COLLECT_C::SensorMsg2Bin(const MyCloud::Ptr pCloud) const
{
    double tt = pCloud->header.stamp / 1000000.0;
    string fileName = std::to_string(tt) + strPrefix_ + ".bin";

    pcl::PointCloud<pcl::PointXYZ> cloud;
    pcl::copyPointCloud(*pCloud, cloud);
    
    pcl::io::savePLYFile(strBinDirPath_ + fileName, cloud);

    std::ofstream out;
    out.open(fileName, std::ios::out | std::ios::binary);
    
    for (int i = 0; i < pCloud->size(); ++i)
    {
        float pointX = pCloud->points[i].x;
        float pointY = pCloud->points[i].y;
        float pointZ = pCloud->points[i].z;
        
        out.write(reinterpret_cast<const char *>(&pointX), sizeof(float));
        out.write(reinterpret_cast<const char *>(&pointY), sizeof(float));
        out.write(reinterpret_cast<const char *>(&pointZ), sizeof(float));
    }
    
    out.close();
    
    usleep(10 * 1000);
    return fileName;
}

/**************************************************************************************
功能描述: google draco encoder
修改记录:
**************************************************************************************/
int LIDAR_DATA_COLLECT_C::DracoEndoder(string fileName)
{
    Options options;
    options.input  = strPlyDirPath_ + fileName;

    fileName = fileName.substr(0, fileName.size() - 4);
    options.output = strDrcDirPath_ + fileName + ".drc";

    options.is_point_cloud = true;

    // 0~30, draco default:11
    options.pos_quantization_bits = 11;

    // 0~10, draco default:7, the highest setting, will have the most compression but worst decompression speed. 
    options.compression_level = 7;

    
    std::unique_ptr<draco::PointCloud> pc;

    auto maybe_pc = draco::ReadPointCloudFromFile(options.input);
    if (!maybe_pc.ok()) {
        ST_LOG_ERR("Failed loading the input point cloud: %s.", maybe_pc.status().error_msg());
        return -1;
    }

    pc = std::move(maybe_pc).value();
    
    if (options.pos_quantization_bits < 0 || options.pos_quantization_bits > 30) {
        ST_LOG_ERR("Position attribute Error.");
        return -1;
    }

    draco::Encoder encoder;

    // Setup encoder options.
    if (options.pos_quantization_bits > 0) {
        encoder.SetAttributeQuantization(draco::GeometryAttribute::POSITION,
                                         options.pos_quantization_bits);
    }
    
    if (options.tex_coords_quantization_bits > 0) {
        encoder.SetAttributeQuantization(draco::GeometryAttribute::TEX_COORD,
                                         options.tex_coords_quantization_bits);
    }
    
    if (options.normals_quantization_bits > 0) {
        encoder.SetAttributeQuantization(draco::GeometryAttribute::NORMAL,
                                         options.normals_quantization_bits);
    }
    
    if (options.generic_quantization_bits > 0) {
        encoder.SetAttributeQuantization(draco::GeometryAttribute::GENERIC,
                                         options.generic_quantization_bits);
    }

    // Convert compression level to speed (that 0 = slowest, 10 = fastest).
    int speed = 10 - options.compression_level;
    encoder.SetSpeedOptions(speed, speed);

    EncodePointCloudToFile(*pc, options.output, &encoder);
    
    return 0;
}
